#pragma config(Sensor, in1,    lineCenter,     sensorLineFollower)
#pragma config(Sensor, in2,    lineLeft,       sensorLineFollower)
#pragma config(Sensor, in3,    lineRight,      sensorLineFollower)
#pragma config(Sensor, in4,    lightStart,     sensorReflection)
#pragma config(Sensor, dgtl1,  touchStart,     sensorTouch)
#pragma config(Sensor, dgtl2,  obstacleDetect, sensorSONAR_inch)
#pragma config(Motor,  port2,           leftDrive,     tmotorVex269, openLoop, reversed)
#pragma config(Motor,  port4,           rightDrive,    tmotorVex269, openLoop)
// *!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

bool checkCenterlight();
bool checkLeftlight();
bool checkRightlight();
bool checkpointLight();
bool checkStartTouch();
bool checkObstacle();

#define STOPPED 0
#define FORWARD 1
#define leftOfLine 2
#define rightOfLine 3
#define BeforeCheckpoint 4
#define AfterCheckpoint 5
#define dodge1 6
#define dodge2 7
#define lookingForObstacle 8
#define notlookingForObstacle 9

task main()
{
  while (!checkStartTouch())
  {
  }
  bool centerSense;
  bool leftSense;
  bool rightSense;
  bool obstacleDetected;
  int robotState = STOPPED;
  int robotPosition = BeforeCheckpoint;
  int isLooking = lookingForObstacle;
  while(true)
  {
    // int test = SensorValue(lineCenter);
    // int test2 = SensorValue(lineLeft);
    // int test3 = SensorValue(lineRight);
    centerSense = checkCenterlight();
    leftSense = checkLeftlight();
    rightSense = checkRightlight();
    obstacleDetected = checkObstacle();
    switch(robotState)
    {
    case STOPPED:
      if (centerSense)
      {
        motor[rightDrive] = 40;
        motor[leftDrive] = 40;
        robotState = FORWARD;
      }
      break;
    case FORWARD:
      if (checkpointLight())
      {
        robotPosition = AfterCheckpoint;
      }
      else if (leftSense)
      {
        robotState = rightOfLine;
        motor[rightDrive] = 30;
        motor[leftDrive] =  -30;
      }
      else if (rightSense)
      {
        robotState = leftOfLine;
        motor[leftDrive] = 30;
        motor[rightDrive] = -30;
      }
      else if (obstacleDetected)
      {
        if (robotPosition == BeforeCheckpoint && isLooking == lookingForObstacle)
        {
          motor[rightDrive] = 60;
          motor[leftDrive] = -60;
          wait1Msec(900);
          motor[rightDrive] = 10;
          motor[leftDrive] = 120;
          wait1Msec(2500);
          motor[rightDrive] = 80;
          motor[leftDrive] = 40;
          robotState = dodge1;
        }
        else
        {
          robotState = dodge2;
        }
      }
      break;
    case leftOfLine:
      if (centerSense)
      {
        robotState = FORWARD;
        motor[rightDrive] = 60;
        motor[leftDrive] = 60;
      }
      break;
    case rightOfLine:
      if (centerSense)
      {
        robotState = FORWARD;
        motor[rightDrive] = 60;
        motor[leftDrive] = 60;
      }
      break;
    case dodge1:
      isLooking = notlookingForObstacle;
      if (centerSense)
      {
        motor[rightDrive] = 70;
        motor[leftDrive] = -50;
        robotState = rightOfLine;
      }
      break;
    case dodge2:
      {
        // motor[rightDrive] = -60;
        // motor[leftDrive] = 60;
        // wait1Msec(1000);
        // motor[rightDrive] = 120;
        // motor[leftDrive] = 20;
        // wait1Msec(2800);
        // motor[rightDrive] = -50;
        // motor[leftDrive] = 50;
        // wait1Msec(300);
        // motor[rightDrive] =-60;
        // motor[leftDrive] = 40;
        // robotState = rightOfLine;
        break;
      }
    }
  }
}
bool checkRightlight()
{
  int change = 1600;
  if (SensorValue(lineRight) > change)
  {
    return false;
  }
  else
  {
    return true;
  }
}
bool checkLeftlight()
{
  int change = 1600;
  if (SensorValue(lineLeft) > change)
  {
    return false;
  }
  else
  {
    return true;
  }
}
bool checkCenterlight()
{
  int change = 1600;
  if (SensorValue(lineCenter) > change)
  {
    return false;
  }
  else
  {
    return true;
  }
}
bool checkpointLight()
{
  int threshold = 150;
  if (SensorValue(lightStart) > threshold)
  {
    return false;
  }
  else
  {
    return true;
  }
}
bool checkStartTouch()
{
  if (SensorValue(touchStart))
  {
    return true;
  }
  else
  {
    return false;
  }
}
bool checkObstacle()
{
  int obstacleDistance = 8;
  if (SensorValue(obstacleDetect) < obstacleDistance && SensorValue(obstacleDetect) != -1)
  {
  return true;
  }
  else
  {
  return false;
  }
}
